#!/usr/bin/env python
#coding=utf-8
from rospkg import RosPack
import sys
from rospy.timer import Rate

from rospy.topics import Publisher
sys.path.append(RosPack().get_path('birl_hardware_interface')+"/script/")
from birl_hardware_interface_v3 import robot
import rospy
from birl_manipulator.srv import *
from birl_manipulator.msg import *
import threading
import time

manipulator = robot()
running_flag = False

def publish_position(Publisher):
    rlock = threading.RLock()
    rlock.acquire()
    position = manipulator.get_joint_position()
    Publisher.publish(position)
    rlock.release()

def handle_target_pose(req):
    manipulator.send_position(req.target_pose)
    return TargetPoseResponse(True)

def manipulator_node():
    manipulator.robot_start()
    manipulator.set_position_mode()
    rospy.init_node('manipulator',anonymous=True)
    service = rospy.Service('manipulator_position_service',TargetPose,handle_target_pose)
    publisher = rospy.Publisher('manipulator_position',robot_feedback,queue_size=10)
    # 受限于CANOpen物理总线的原因 消息发布频率降低
    rate = rospy.Rate(2) #2hz
    # when the node is running , it will publish the position of each joint 
    while not rospy.is_shutdown():
        thread_ = threading.Thread(target=publish_position,args=(publisher,))
        thread_.start()
        thread_.join()
        rate.sleep()
    rospy.spin()

def manipulator_back2zero():
    # 在机器人停止通讯之前 让机器人回到零点
    zero_pos = [0,0,0,0,0]
    manipulator.send_position(zero_pos)

    # 适当延时保证机器人能回到原点
    current_joint_pos = manipulator.get_joint_position()
    max_ =max(map(abs,current_joint_pos))
    time.sleep(round(max_/5)+2)
    manipulator.robot_stop()
    

if __name__ == "__main__":
    try:
        manipulator_node()
        rospy.on_shutdown(manipulator_back2zero)
    except rospy.ROSInterruptException as e:
        print("%s"%e)
        pass


